package com.turtle3d.turtle;

import java.util.Stack;

import javax.vecmath.Matrix4f;
import javax.vecmath.Vector3f;

import com.turtle3d.visualizator.Turtle3DVisualisator;


public class Turtle3DImplementation extends Turtle3DCore implements Turtle3D {
	
	private Stack<Matrix4f> stack = new Stack<Matrix4f>();
	private int depth=0;
	private int maximalDepth=0;
	private Stack<Integer> depthStack = new Stack<Integer>();
	
	public Turtle3DImplementation(){
				
	}
	
	/* (non-Javadoc)
	 * @see turtle3d.Turtle3D#forward(float)
	 */
	public void forward(float distance){
		
		Matrix4f temp = new Matrix4f();
		temp.setIdentity();
		
		temp.setTranslation(new Vector3f(0,distance,0));
		
		depth++;
		if(depth>maximalDepth)
			maximalDepth=depth;
		transform(temp);		
	}
	
	/* (non-Javadoc)
	 * @see turtle3d.Turtle3D#turn(float)
	 */
	public void turn(float angle){
		Matrix4f temp = new Matrix4f();
		angle=(float)(-Math.PI)*angle/180f;
		temp.setIdentity();
		temp.rotZ(angle);
		//temp.setRotation(new AxisAngle4f(0,1,0,angle));
		transform(temp);
	}
	
	/* (non-Javadoc)
	 * @see turtle3d.Turtle3D#roll(float)
	 */
	public void roll(float angle){
		Matrix4f temp = new Matrix4f();
		angle=(float)(-Math.PI)*angle/180f;
		temp.setIdentity();
		temp.rotY(angle);
		//temp.setRotation(new AxisAngle4f(0,1,0,angle));
		transform(temp);
	}
	
	/* (non-Javadoc)
	 * @see turtle3d.Turtle3D#pitch(float)
	 */
	public void pitch(float angle){
		
	}
	
	/* (non-Javadoc)
	 * @see turtle3d.Turtle3D#push()
	 */
	public void push(){		
		stack.push(new Matrix4f(getTransformMatrix()));
		depthStack.push(depth);
	}
	
	/* (non-Javadoc)
	 * @see turtle3d.Turtle3D#pop()
	 */
	public void pop(){
		setTransformMatrix(stack.pop());
		depth = depthStack.pop();
	}
	
	/* (non-Javadoc)
	 * @see turtle3d.Turtle3D#getHistory()
	 */
	/*public Collection<Matrix4f> getHistory(){
		
	}*/		
		
	public static void main(String[] args) {
		// TODO Auto-generated method stub
				
		
		Turtle3D t= new Turtle3DImplementation();
		t.forward(1);		
		//System.out.println(t.getTransformMatrix());
		t.turn(90);
		//System.out.println(t.getTransformMatrix());
		t.forward(1);
		//System.out.println(t.getTransformMatrix());
		t.turn(-90);
		t.forward(1);
		t.turn(-90);
		t.forward(1);
		t.turn(90);
		t.forward(1);
		
		//Turtle3DVisualisator.createBranchGroup(t);
		
	}

	public int getDepth()
	{
		return depth;
	}

	public int getMaximalDepth()
	{
		return maximalDepth;
	}

	/* (non-Javadoc)
	 * @see turtle3d.Turtle3D#getTransformMatrix()
	 */	

}
